#!/usr/bin/env python

import roslib; roslib.load_manifest('rosqwerk')
import rospy

import sys
import threading
import traceback
from rosqwerk.msg import InputStates
from rosqwerk.msg import MotorStates
from rosqwerk.msg import ServoStates
from rosqwerk.msg import ServosControl
from rosqwerk.msg import MotorsControl
from rosqwerk.msg import DigitalOutputsControl

import Ice
from TeRK import ServoMode,ServoCommand,ServoControllerPrx,MotorMode,MotorCommand,MotorControllerPrx,DigitalOutCommand,DigitalOutControllerPrx,QwerkPrx


class QwerkNode():    
    def runQwerkNode(self):
        """
        Initialize the Qwerk node
        """        
        rospy.init_node('qwerk', anonymous=True, log_level=rospy.DEBUG)
        qwerkHostname = rospy.get_param('~qwerk_hostname', '192.168.0.7') 
        publishFrequency = rospy.get_param('~publish_frequency', 50)        
        
        ice = Ice.initialize(sys.argv)       
        try:
            rospy.loginfo("Connecting to Qwerk on %s..." % qwerkHostname)
            qwerkB = ice.stringToProxy("'::TeRK::TerkUser':tcp -p 10101 -h %s" % qwerkHostname)
            qwerkB.ice_ping() # will throw if can't connect           
            rospy.loginfo("Connected")            
            #supportedServices=qwerk.getSupportedServices()
            #print "qwerk services:", supportedServices
            qwerkProxy = QwerkPrx.checkedCast(qwerkB)                  
            QwerkStatePublisher(qwerkProxy,publishFrequency).start()
            
            servoServiceB=ice.stringToProxy("'::TeRK::ServoController':tcp -p 10101 -h %s" % qwerkHostname)
            servoServiceProxy = ServoControllerPrx.checkedCast(servoServiceB)    
            
            motorServiceB=ice.stringToProxy("'::TeRK::MotorController':tcp -p 10101 -h %s" % qwerkHostname)
            motorServiceProxy = MotorControllerPrx.checkedCast(motorServiceB)     
            
            digitalOutServiceB=ice.stringToProxy("'::TeRK::DigitalOutController':tcp -p 10101 -h %s" % qwerkHostname)
            digitalOutServiceProxy = DigitalOutControllerPrx.checkedCast(digitalOutServiceB)     
                    
            QwerkServicesListeners(servoServiceProxy,motorServiceProxy,digitalOutServiceProxy).start()            

            rospy.spin()        
        finally:
            try:
                qwerkProxy.emergencyStop()
                ice.destroy()
            except:
                pass
    
    def shutdown(self):
        try:
            QwerkStatePublisher.stop
            QwerkServicesListeners.stop
        except:
            pass
        
class RunningAverage:
    def __init__(self, nbSamples):     
        self.meanValue=0.0
        self.nbSamples=0.0+nbSamples
    
    def update(self,rawValue):
        self.meanValue = (rawValue / self.nbSamples) + (1.0 - 1.0 / self.nbSamples) * self.meanValue
    
    def getValue(self):
        return self.meanValue
    
                
class QwerkStatePublisher(threading.Thread):
    def __init__(self, qwerkProxy, publishFrequency):       
        threading.Thread.__init__(self)
        self.inputStatesPublisher = rospy.Publisher('qwerk/input_states', InputStates)              
        self.motorStatesPublisher = rospy.Publisher('qwerk/motor_states', MotorStates)              
        self.servoStatesPublisher = rospy.Publisher('qwerk/servo_states', ServoStates)              
        self.publishRate = rospy.Rate(publishFrequency)    
        self.qwerkProxy=qwerkProxy    
        
        self.motorCurrentsAverage=[]  
        for i in range(len(MotorStates().motorPositions)):
            self.motorCurrentsAverage.append(RunningAverage(15))   
        
    def run(self):
        rospy.loginfo("Qwerk state publisher starting")
        self.threadName = "Qwerk State Publisher"        
        while not rospy.is_shutdown():
            qwerkState=self.qwerkProxy.getState()
        
            inputs = InputStates()
            inputs.batteryVoltage=qwerkState.battery.batteryVoltage
            for i in range(len(inputs.digitalInStates)):
                inputs.digitalInStates[i]=qwerkState.digitalIn.digitalInStates[i]
            for i in range(len(inputs.analogInValues)):
                inputs.analogInValues[i]=qwerkState.analogIn.analogInValues[i]            
            inputs.header.stamp = rospy.Time.now()
            self.inputStatesPublisher.publish(inputs)      

            motors = MotorStates()
            for i in range(len(motors.motorPositions)):
                motors.motorPositions[i]=qwerkState.motor.motorPositions[i]
                motors.motorVelocities[i]=qwerkState.motor.motorVelocities[i]
                self.motorCurrentsAverage[i].update(abs(qwerkState.motor.motorCurrents[i]))
                motors.motorCurrents[i]=self.motorCurrentsAverage[i].getValue()
                motors.motorDone[i]=qwerkState.motor.motorDone[i]            
            motors.header.stamp = rospy.Time.now()
            self.motorStatesPublisher.publish(motors)      
            
            servos = ServoStates()
            for i in range(len(servos.servoPositions)):
                servos.servoPositions[i]=qwerkState.servo.servoPositions[i]                      
            servos.header.stamp = rospy.Time.now()
            self.servoStatesPublisher.publish(servos)      

            self.publishRate.sleep()    
            
class QwerkServicesListeners(threading.Thread):   
    def __init__(self, servoServiceProxy,motorServiceProxy,digitalOutServiceProxy):        
        threading.Thread.__init__(self)
        self.servoServiceProxy=servoServiceProxy   
        self.servocmd=ServoCommand()
        self.servocmd.servoPositions=[]
        self.servocmd.servoModes=[]
        self.servocmd.servoMask=[]
        self.servocmd.servoSpeeds=[]
        for i in range(16):
            self.servocmd.servoPositions.append(127)
            self.servocmd.servoModes.append(ServoMode.ServoMotorPositionControl)
            self.servocmd.servoMask.append(True)
            self.servocmd.servoSpeeds.append(sys.maxint)
        
        self.motorServiceProxy=motorServiceProxy   
        self.motorcmd=MotorCommand()
        self.motorcmd.motorMask=[]
        self.motorcmd.motorPositions=[]
        self.motorcmd.motorVelocities=[]
        self.motorcmd.motorAccelerations=[]
        self.motorcmd.motorModes=[]
        for i in range(4):
            self.motorcmd.motorPositions.append(0)
            self.motorcmd.motorAccelerations.append(sys.maxint)
            self.motorcmd.motorMask.append(True)
            self.motorcmd.motorVelocities.append(sys.maxint)      
            self.motorcmd.motorModes.append(MotorMode.MotorOff);
            
        self.digitalOutServiceProxy=digitalOutServiceProxy   
        self.digitaloutcmd=DigitalOutCommand()    
                    
    def run(self):
                    
        def servosServiceCallback(msg):       
            rospy.logdebug("From: " + rospy.get_caller_id() + ". Servo request:" + str(msg))
            for i in range(len(msg.servoPositions)):
                self.servocmd.servoPositions[i]=msg.servoPositions[i]
                self.servocmd.servoMask[i]=(msg.servoPositions[i]>=0)
            self.servoServiceProxy.execute(self.servocmd)
            return    
        
        def motorsServiceCallback(msg):       
            rospy.logdebug("From: " + rospy.get_caller_id() + ". Motor request:" + str(msg))
            for i in range(len(msg.motorPositions)):
                self.motorcmd.motorMask[i]=(msg.motorVelocities[i] != MotorsControl.INVALID_VALUE)
                if (msg.motorPositions[i]==MotorsControl.INVALID_VALUE):
                    if (msg.motorVelocities[i]==0):
                        self.motorcmd.motorModes[i]=MotorMode.MotorOff
                    else:
                        self.motorcmd.motorModes[i]=MotorMode.MotorSpeedControl
                        self.motorcmd.motorPositions[i]=0
                        self.motorcmd.motorVelocities[i]=msg.motorVelocities[i]
                else:
                    self.motorcmd.motorModes[i]=MotorMode.MotorPositionControl
                    self.motorcmd.motorPositions[i]=msg.motorPositions[i]
                    self.motorcmd.motorVelocities[i]=msg.motorVelocities[i]
            self.motorServiceProxy.execute(self.motorcmd)
            rospy.logdebug("Sending qwerk motor command %s" % self.motorcmd)
            return    
        
        def digitalOutputsServiceCallback(msg):       
            rospy.logdebug("From: " + rospy.get_caller_id() + ". Digital out request:" + str(msg))
            self.digitaloutcmd.digitalOutMask=msg.digitalOutMask
            self.digitaloutcmd.digitalOutValues=msg.digitalOutStates
            self.digitalOutServiceProxy.execute(self.digitaloutcmd)
            return            
        
        self.threadName = "Qwerk Services Listener" 
        rospy.loginfo("Qwerk servos service listener starting")
        rospy.Subscriber("/qwerk/servos_control", ServosControl, servosServiceCallback)  
        rospy.loginfo("Qwerk motors service listener starting")
        rospy.Subscriber("/qwerk/motors_control", MotorsControl, motorsServiceCallback)        
        rospy.loginfo("Qwerk digital outputs service listener starting")
        rospy.Subscriber("/qwerk/digitaloutputs_control", DigitalOutputsControl, digitalOutputsServiceCallback)        
        try:
            rospy.spin()
        except rospy.ROSInterruptException:
            rospy.loginfo("Shutdown request. Shutting down Qwerk services listeners.")
            exit(0)                        
            
if __name__ == '__main__':
    qwerkNode = QwerkNode()
    try:
        qwerkNode.runQwerkNode()        
    except rospy.ROSInterruptException:
        rospy.loginfo("ROS Shutdown Request.")
    except KeyboardInterrupt, e:
        rospy.loginfo("Received keyboard interrupt.")
    except:
        excType, excValue, excTraceback = sys.exc_info()[:3]
        traceback.print_exception(excType, excValue, excTraceback)
    finally:
        if (qwerkNode is not None):
            qwerkNode.shutdown()
        rospy.loginfo("Exiting Qwerk node.")
        sys.exit(0)
